import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.*;

import java.util.ArrayList;

public class Master {
	private float distanceToWall;
	TachoPilot p;
	int angle = 45;
	Wallfollow wf;
	static Master m = new Master();
	private BarCode lastBarCode;
	Thread t;
	BarCodeFSMThread barcodelezer;
	Driver goldieDriver;
	SimpleNavigator navigator;
	BTConnection connection;
	DataInputStream dataIn;
	DataOutputStream dataOut;
	private boolean barcodeDetected;
	final float moveSpeed = 30f;
	boolean trackCompleted = false;
	int currentBarCodeIndex=-1;
	int trackSimulationLengthRemaining= 0;

	int posx = 0;
	int posy = 0;
	int richting = 0;
	int panelNumber = 0;
	ArrayList<BarCode> track = new ArrayList<BarCode>();

	class DataThread implements Runnable {
		//		private DataOutputStream threadDataOut;
		//		public DataThread(DataOutputStream dataOut) {
		//			this.threadDataOut = dataOut;
		//		}
		public void run() {
			while(true){
				try {
					if(barcodeDetected) {
						dataOut.writeInt(0);
						if(lastBarCode == BarCode.RECHTDOOR)
							dataOut.writeInt(0);
						else if(lastBarCode == BarCode.LINKS)
							dataOut.writeInt(1);
						else if(lastBarCode == BarCode.RECHTS)
							dataOut.writeInt(2);
						else dataOut.writeInt(-1);
						barcodeDetected = false;
					}
					dataOut.flush();
				} catch (IOException e) {

				}
				try {
					Thread.sleep(200);
				} catch (InterruptedException e) {
				}


			}

		}



	}

	public static void main(String[] args)
	{
		// Nieuwe muurvolger starten.
		m.go();
	}

	public Master(){
		System.out.println("Choose Bluetooth");
		int id = Button.waitForPress();
		if(id == Button.ID_LEFT){
			RConsole.openBluetooth(0);
		}
		else if(id == Button.ID_RIGHT){
			connect();
			Thread d = new Thread(new DataThread());
			d.start();
		}
		p = new TachoPilot(5.52012f, 11.14f, Motor.A, Motor.C,true);
		p.setMoveSpeed(moveSpeed);

		goldieDriver = new Driver(p,50, moveSpeed);
		t = new Thread(new DistanceThread());
		barcodelezer = new BarCodeFSMThread(this);
		
		track.add(BarCode.RECHTDOOR);
	}

	private void setTrackCompleted(boolean trackCompleted){
		System.out.println("track completed: " + trackCompleted);
		this.trackCompleted = trackCompleted;
	}

	private void sleep(int milliSeconds){
		try{
			Thread.sleep(milliSeconds);
		}catch(InterruptedException e){
		}
	}
	private void go(){
		//System.out.println("add commands");
		goldieDriver.addCommand(new Command(0, 40, false, CommandEnum.TRAVEL));
		//		goldieDriver.addCommand(new Command(0, 20, true, CommandEnum.TRAVEL));
		//		goldieDriver.addCommand(new Command(40, 90, true, CommandEnum.STEER));
		//		goldieDriver.addCommand(new Command(-40, 90, true, CommandEnum.STEER));
		//		goldieDriver.addCommand(new Command(0, 10, true, CommandEnum.TRAVEL));

		t.start();
		wf = new Wallfollow(this, goldieDriver, false, moveSpeed);
		wf.pause();
		barcodelezer.start();


		//p.travel(200,true);


	}

	public void barCodeDetected (BarCode bc){
		//System.out.println("barcode detected: " + bc.toString());
		barcodeDetected = true;
		if (!trackCompleted)
			nextPanel(bc);
		this.lastBarCode = bc;
		//		Thread barCodeThread = new Thread(pbc);
		//		barCodeThread.start();
		processBarcode(bc);

	}

	private void processBarcode(BarCode bc){
		float straightDistance= 15f;
		Command straight = new Command(0, straightDistance, true, CommandEnum.TRAVEL);
		BarCode barCode = bc;
		if(trackCompleted){
			currentBarCodeIndex++;
			if(getBarCodeAt(currentBarCodeIndex) != bc && getBarCodeAt(currentBarCodeIndex) == BarCode.RECHTDOOR){
				trackSimulationBroken();
				System.out.println("track simulation was broken at barcodeIndex: " + currentBarCodeIndex);
			}else{
				if(trackSimulationLengthRemaining<1){
					System.out.println("current barcode: " + getBarCodeAt(currentBarCodeIndex));
					simulateNewTrackSequence(bc);
				}
				else
					trackSimulationLengthRemaining--;
			}
		}
		if (!trackCompleted){
			if (barCode == BarCode.RECHTDOOR)
				wf.restart();
			else if (barCode==BarCode.RECHTS){
				wf.pause();
				changeWallSide(true);
				goldieDriver.addCommand(straight);
				goldieDriver.addCommand(new Command(-50, 90, true, CommandEnum.STEER));
				goldieDriver.addCommand(straight);
				wf.restart();
			}
			else if (barCode==BarCode.LINKS){
				wf.pause();
				changeWallSide(false);
				goldieDriver.addCommand(straight);
				goldieDriver.addCommand(new Command(50, 90, true, CommandEnum.STEER));
				goldieDriver.addCommand(straight);
				wf.restart();
			}
			else if (barCode==BarCode.WITTELIJN){
				wf.restart();
			}
			else if (barCode==BarCode.ZWARTELIJN){
				wf.restart();
			}
		}
	}

	private void simulateNewTrackSequence(BarCode bc) {
		final float distanceToWall=25f;
		switch (bc){
		case RECHTDOOR:
			int cnt = countStraightTracksAfterIndex(currentBarCodeIndex);
			BarCode bcAfterRechtdoor = getBarCodeAt(currentBarCodeIndex + cnt);
			int side=0;
			if(bcAfterRechtdoor==BarCode.LINKS){
				side = 1;
			}
			else if(bcAfterRechtdoor == BarCode.RECHTS){
				side = -1;
			}
			else
				throw new IllegalStateException("rechts of links barcode verwacht na rechtdoor");
			if (cnt==1){
				setWallFollow(distanceToWall,side);
				trackSimulationLengthRemaining=0;
			}
			else if(cnt>1){
				float distance = distanceToWall + (getDistance()-distanceToWall)/cnt;
				setWallFollow(distance, side);
				trackSimulationLengthRemaining=0;
			}
			break;

		case RECHTS:
			switch (getBarCodeAt(currentBarCodeIndex+1)){
			case RECHTS:
				wf.pause();
				goldieDriver.addCommand(new Command(-50, 40, true, CommandEnum.STEER));
				wf.restart();
				trackSimulationLengthRemaining=0;
				break;
			case RECHTDOOR:
				wf.pause();
				goldieDriver.addCommand(new Command(-50,40, true, CommandEnum.STEER));
				setWallFollow(60, 1);
				trackSimulationLengthRemaining=0;
				break;
			case LINKS:
				switch (getBarCodeAt(currentBarCodeIndex+2)){
				case RECHTS:
					setWallFollow(distanceToWall,-1);
					trackSimulationLengthRemaining=1;
					break;
				case RECHTDOOR:
					setWallFollow(60,-1);
					trackSimulationLengthRemaining=1;
					break;
				case LINKS:
					setWallFollow(60,-1);
					trackSimulationLengthRemaining=1;
					break;
				}
				break;

			}


		case LINKS:
			switch (getBarCodeAt(currentBarCodeIndex+1)){
			case RECHTS:
				wf.pause();
				goldieDriver.addCommand(new Command(50, 40, true, CommandEnum.STEER));
				wf.restart();
				trackSimulationLengthRemaining=0;
				break;
			case RECHTDOOR:
				wf.pause();
				goldieDriver.addCommand(new Command(50, 40, true, CommandEnum.STEER));
				setWallFollow(60, -1);
				trackSimulationLengthRemaining=0;
				break;
			case LINKS:
				switch (getBarCodeAt(currentBarCodeIndex+2)){
				case RECHTS:
					setWallFollow(distanceToWall,1);
					trackSimulationLengthRemaining=1;
					break;
				case RECHTDOOR:
					setWallFollow(60,1);
					trackSimulationLengthRemaining=1;
					break;
				case LINKS:
					setWallFollow(60,1);
					trackSimulationLengthRemaining=1;
					break;
				}
				break;

			}

		}


	}


	private int countStraightTracksAfterIndex(int currentBarCodeIndex2) {
		int cnt=0;
		while(getBarCodeAt(currentBarCodeIndex2+cnt)==BarCode.RECHTDOOR){
			cnt++;
			if(cnt>20){
				throw new IllegalStateException();
			}
		}
		return cnt;
	}

	private void trackSimulationBroken() {
		currentBarCodeIndex=0;
		setTrackCompleted(false);
		resetTrack();
		wf.setDistance(40f);
	}
	
	private void resetTrack(){
		panelNumber=0;
		richting=0;
		posx=0;
		posy=0;
		track.clear();
		track.add(BarCode.RECHTDOOR);
	}

	public BarCode getBarCode(){
		return this.lastBarCode;
	}


	public boolean lineDetected(){

		return barcodelezer.readingStrip;
	}

	public void driveForward(){
		if(!trackCompleted){
			wf.pause();
			goldieDriver.addCommand(new Command(0, 30, false, CommandEnum.TRAVEL));
		}
	}


	private void changeWallSide(boolean left){
		if(wf.getSideLeft()!=left){
			//wf.pause();
			Motor.B.rotate((int)(-wf.getSide()*(180)),true);
			wf.setSideLeft(left);
		}
	}

	private void nextPanel(BarCode newBar){
		panelNumber++;
		//Richtingen: 0 = rechts
		// 1 = onder
		// 2 = links
		// 3 = boven
		if (richting == 0)
			posx++;
		else if (richting == 1)
			posy--;
		else if (richting == 2)
			posx--;		
		else if (richting == 3)
			posy++;

		if(newBar == BarCode.RECHTDOOR){
			//richting = richting;
		}
		else if (newBar == BarCode.LINKS){
			richting = (richting+3)%4;
		}
		else if (newBar == BarCode.RECHTS){
			richting = (richting+1)%4;
		}

		
		System.out.println("posx: " + posx +" posy: " + posy);

		if(posx == 0 && posy == 0){
			setTrackCompleted(true);
			panelNumber = 0;
			track.set(0,newBar);
		}
		else{
			track.add(newBar);
		}
	}


	//getter maken met modulo-index
	public BarCode getBarCodeAt(int panelNum){
		int panel = panelNum%(track.size());
		return track.get(panel);
	}

	private void setWallFollow(float distance, int side){

		boolean left= (side==1);
		goldieDriver.addCommand(new Command(0, 10, true, CommandEnum.TRAVEL));
		changeWallSide(left);
		wf.setDistance(distance);
		wf.restart();
	}

	class DistanceThread implements Runnable {
		public void run(){
			final UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
			float[] distances;
			distances = new float[10];
			while(true){
				for(int i = 9; i>0;i--){
					distances[i]=distances[i-1];
				}

				distances[0] = sonic.getRange(); 

				try{
					Thread.sleep(2);
				}catch(InterruptedException e){
				}

				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				float smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest && distances[i] > 5)
						smallest = distances[i];
				}

				distanceToWall = smallest;
			}
		}
	}


	public float getDistance(){
		return this.distanceToWall;
	}

	public float getTravelDistance(){

		return p.getTravelDistance();
	}
	/**
	 * Estabish bluetooth connection to mission control
	 */
	public void connect()
	{
		LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		connection = Bluetooth.waitForConnection(); // this method is very patient. 
		LCD.clear();
		LCD.drawString("Connected", 0, 0);
		dataIn = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream();
	}

}
